#include "U2Matrix4.h"

//#include "U2Vector3.h"
//#include "U2Matrix3.h"


U2EG_NAMESPACE_USING


const U2Matrix4 U2Matrix4::ZERO(
    0, 0, 0, 0,
    0, 0, 0, 0,
    0, 0, 0, 0,
    0, 0, 0, 0 );

const U2Matrix4 U2Matrix4::ZEROAFFINE(
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 1 );

const U2Matrix4 U2Matrix4::IDENTITY(
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1 );

const U2Matrix4 U2Matrix4::CLIPSPACE2DTOIMAGESPACE(
      0.5,  0,  0, 0.5, 
      0, -0.5,  0, 0.5, 
      0,    0,  1,   0,
      0,    0,  0,   1);

//-----------------------------------------------------------------------
inline static u2real
    MINOR(const U2Matrix4& m, const size_t r0, const size_t r1, const size_t r2, 
							const size_t c0, const size_t c1, const size_t c2)
{
    return m[r0][c0] * (m[r1][c1] * m[r2][c2] - m[r2][c1] * m[r1][c2]) -
        m[r0][c1] * (m[r1][c0] * m[r2][c2] - m[r2][c0] * m[r1][c2]) +
        m[r0][c2] * (m[r1][c0] * m[r2][c1] - m[r2][c0] * m[r1][c1]);
}
//-----------------------------------------------------------------------
U2Matrix4 U2Matrix4::adjoint() const
{
    return U2Matrix4( MINOR(*this, 1, 2, 3, 1, 2, 3),
        -MINOR(*this, 0, 2, 3, 1, 2, 3),
        MINOR(*this, 0, 1, 3, 1, 2, 3),
        -MINOR(*this, 0, 1, 2, 1, 2, 3),

        -MINOR(*this, 1, 2, 3, 0, 2, 3),
        MINOR(*this, 0, 2, 3, 0, 2, 3),
        -MINOR(*this, 0, 1, 3, 0, 2, 3),
        MINOR(*this, 0, 1, 2, 0, 2, 3),

        MINOR(*this, 1, 2, 3, 0, 1, 3),
        -MINOR(*this, 0, 2, 3, 0, 1, 3),
        MINOR(*this, 0, 1, 3, 0, 1, 3),
        -MINOR(*this, 0, 1, 2, 0, 1, 3),

        -MINOR(*this, 1, 2, 3, 0, 1, 2),
        MINOR(*this, 0, 2, 3, 0, 1, 2),
        -MINOR(*this, 0, 1, 3, 0, 1, 2),
        MINOR(*this, 0, 1, 2, 0, 1, 2));
}
//-----------------------------------------------------------------------
u2real U2Matrix4::determinant() const
{
    return m[0][0] * MINOR(*this, 1, 2, 3, 1, 2, 3) -
        m[0][1] * MINOR(*this, 1, 2, 3, 0, 2, 3) +
        m[0][2] * MINOR(*this, 1, 2, 3, 0, 1, 3) -
        m[0][3] * MINOR(*this, 1, 2, 3, 0, 1, 2);
}
//-----------------------------------------------------------------------
U2Matrix4 U2Matrix4::inverse() const
{
    u2real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
    u2real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
    u2real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
    u2real m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];

    u2real v0 = m20 * m31 - m21 * m30;
    u2real v1 = m20 * m32 - m22 * m30;
    u2real v2 = m20 * m33 - m23 * m30;
    u2real v3 = m21 * m32 - m22 * m31;
    u2real v4 = m21 * m33 - m23 * m31;
    u2real v5 = m22 * m33 - m23 * m32;

    u2real t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
    u2real t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
    u2real t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
    u2real t30 = - (v3 * m10 - v1 * m11 + v0 * m12);

    u2real invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);

    u2real d00 = t00 * invDet;
    u2real d10 = t10 * invDet;
    u2real d20 = t20 * invDet;
    u2real d30 = t30 * invDet;

    u2real d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    u2real d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    u2real d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    u2real d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    v0 = m10 * m31 - m11 * m30;
    v1 = m10 * m32 - m12 * m30;
    v2 = m10 * m33 - m13 * m30;
    v3 = m11 * m32 - m12 * m31;
    v4 = m11 * m33 - m13 * m31;
    v5 = m12 * m33 - m13 * m32;

    u2real d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    u2real d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    u2real d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    u2real d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    v0 = m21 * m10 - m20 * m11;
    v1 = m22 * m10 - m20 * m12;
    v2 = m23 * m10 - m20 * m13;
    v3 = m22 * m11 - m21 * m12;
    v4 = m23 * m11 - m21 * m13;
    v5 = m23 * m12 - m22 * m13;

    u2real d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
    u2real d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
    u2real d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
    u2real d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

    return U2Matrix4(
        d00, d01, d02, d03,
        d10, d11, d12, d13,
        d20, d21, d22, d23,
        d30, d31, d32, d33);
}
//-----------------------------------------------------------------------
U2Matrix4 U2Matrix4::inverseAffine(void) const
{
    assert(isAffine());

    u2real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2];
    u2real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2];

    u2real t00 = m22 * m11 - m21 * m12;
    u2real t10 = m20 * m12 - m22 * m10;
    u2real t20 = m21 * m10 - m20 * m11;

    u2real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2];

    u2real invDet = 1 / (m00 * t00 + m01 * t10 + m02 * t20);

    t00 *= invDet; t10 *= invDet; t20 *= invDet;

    m00 *= invDet; m01 *= invDet; m02 *= invDet;

    u2real r00 = t00;
    u2real r01 = m02 * m21 - m01 * m22;
    u2real r02 = m01 * m12 - m02 * m11;

    u2real r10 = t10;
    u2real r11 = m00 * m22 - m02 * m20;
    u2real r12 = m02 * m10 - m00 * m12;

    u2real r20 = t20;
    u2real r21 = m01 * m20 - m00 * m21;
    u2real r22 = m00 * m11 - m01 * m10;

    u2real m03 = m[0][3], m13 = m[1][3], m23 = m[2][3];

    u2real r03 = - (r00 * m03 + r01 * m13 + r02 * m23);
    u2real r13 = - (r10 * m03 + r11 * m13 + r12 * m23);
    u2real r23 = - (r20 * m03 + r21 * m13 + r22 * m23);

    return U2Matrix4(
        r00, r01, r02, r03,
        r10, r11, r12, r13,
        r20, r21, r22, r23,
          0,   0,   0,   1);
}


